% 针孔相机模型 不包含径向畸变
function [K,R,T,M, rho] = PinholeCameraCalibration(points2D_Homo, points3D_Homo)
    % 构造线性 齐次方程组
    pointsNums = max(size(points2D_Homo));
    % 初始化系数矩阵 大小 没对点包含两个信息 因此2*pointsNums dim
    coeffMat = zeros(2*pointsNums, 12);
    for i = 1:2:2*pointsNums
        ui = points2D_Homo((i-1)/2+1,1); %二位像素点 x
        vi = points2D_Homo((i-1)/2+1,2); %二位像素点 y
        Pi = points3D_Homo((i-1)/2+1,:); %三维点齐次坐标
        coeffMat(i,:) = [Pi, 0 0 0 0, -ui.*Pi];
        coeffMat(i+1,:) = [0 0 0 0, Pi,  -vi.*Pi];
    end
    % 计算齐次线性方程组最优解
    [~,~, vm] = svd(coeffMat, 0);
    M = reshape(vm(:, end), 4, 3)'; %投影矩阵 
    %M = M/norm(M);
    % 分解出A b的值
    a1 = M(1,1:3)';
    a2 = M(2,1:3)';
    a3 = M(3,1:3)';
    b = M(:,4);
    b1 = M(1,4);
    b2 = M(2,4);
    b3 = M(3,4);

    % 内参数估计
    % 分解出比例系数 rho   
    rho = 1 / norm(a3);     %******
    % 分解出 u v 
    u = rho^2 * (a1' * a3);
    v = rho^2 * (a2' * a3);
    % 分解出theta 像素角
    a1_a3 = AntisymmetricMat(a1) * a3;
    a2_a3 = AntisymmetricMat(a2) * a3;
    cosTheta = - (a1_a3' * a2_a3) / (norm(a1_a3) + norm(a2_a3));
    theta = acos(cosTheta);
    % 分解出alpha beta
    alpha = rho^2 * (norm(a1_a3)) * sin(theta);
    beta = rho^2 * (norm(a2_a3)) * sin(theta);

    K = [alpha, -alpha * (cos(theta) / sin(theta)), u;
         0, beta/sin(theta), v;
         0,0,1];

    % 外参数估计
    r1 = a2_a3 ./ (norm(a2_a3));
    r3 = a3 ./ norm(a3);  %******
    r2 = AntisymmetricMat(r3) * r1;

    R = [r1'; r2'; r3'];
    T = rho .* K^(-1)*b;
end

%计算矢量的反对称矩阵 输入列矢量 corss内置函数一样
function output = AntisymmetricMat(input)
    output = zeros(3,3);
    output(1,2) = -input(3);
    output(1,3) = input(2);
    output(2,3) = -input(1);
    output = output-output';
end
